home *** CD-ROM | disk | FTP | other *** search
/ Tech Arsenal 1 / Tech Arsenal (Arsenal Computer).ISO / tek-04 / glovdriv.zip / THEGLOVE.C < prev   
C/C++ Source or Header  |  1992-02-17  |  11KB  |  444 lines
  1. /**********************************************************************
  2.  
  3.   Originally "power.c" (c) manfredo 9/91 (manfredo@opal.cs.tu-berlin.de)
  4.   Developed on an ATARI 1040ST with TC 1.1 using a logic analyzer to get
  5.   the correct timings.
  6.  
  7. **********************************************************************/
  8. /*********************************************************************
  9.               ported to PC compatibles by
  10.                  Greg Alt 10/91
  11.  
  12.                 galt@peruvian.utah.edu
  13.              or galt@es.dsd.com
  14.  
  15. **********************************************************************/
  16. /*********************************************************************
  17.  
  18.  Substantially rewritten by Dave Stampe (c) 1991: PWRFILT.C
  19.   No cash, no warranty, no flames.
  20.   This stuff works great, so gimme credit.
  21.  
  22.  Goals <achieved> were:
  23.  
  24.   Higher speed, smaller code.
  25.   Polled operation is now possible.
  26.   Graphics test (VGA)
  27.   Noise reduction added, gets rid of 99.5% of noise with NO DELAY!
  28.  
  29.   This runs on a 486/25 with an i/o card. 
  30.   Someone should adapt it for the usual printer port adapter.
  31.   It was compiled with Turbo C++ 2.0 but will probably
  32.   work on any Turbo C directly. MSC will need library calls checked.
  33.  
  34.  
  35.  dstamp@watserv1.uwaterloo.ca            17/10/91
  36. **********************************************************************/
  37.  
  38. #include <dos.h>
  39. #include <bios.h>
  40. #include <stdio.h>
  41. #include <conio.h>
  42. #include <graphics.h>
  43.  
  44. int gdriver = VGA;        /* for graphics plot and cursor */
  45. int gmode = VGAHI;
  46.  
  47. #define XHYST 2            /* hysterisis for X, Y low noise reduction */
  48. #define YHYST 2            /* 2 eliminates +/-3 quanta of noise */
  49.  
  50. #define XACC 8                  /* X, Y maximum accel/decel level. Should */
  51. #define YACC 8            /* be 6-10, but too high limits gesturing */
  52.  
  53. #define XXTEND 2        /* stretches deglitching time */
  54. #define YXTEND 1
  55.  
  56. #define N 1               /* delay scaled by N/D <CHANGED> */
  57. #define D 1             /* these are 1,1 for 486 PC with i/o card */
  58. #define INPORT  0x279           /* i/o port addresses <CHANGED> */
  59. #define OUTPORT 0x278
  60.  
  61. /* bits for i/o ports <CHANGED> */
  62.  
  63. #define        GDATA           0x10    /* PG data in */
  64. #define        GLATCH          0x02    /* PG latch out */
  65. #define        GCLOCK          0x01    /* PG clock out */
  66. #define        GCLOLAT         0x03    /* clock + latch */
  67.  
  68. /* delay values for sending and sampling data <CHANGED> */
  69.  
  70. #define     D2BYTES      150      /* delay between 2 bytes = 96 us  */
  71. #define     D2BITS       6        /* delay between 2 bits = 3 us    */
  72. #define     D2SLOW       8000     /* intertest delay = 2000-4000 us */
  73.  
  74. /* Delay timing: may not work in some IBM C's due to problems with LONGs */
  75.  
  76. void fdelay(unsigned int val)
  77. {
  78.  long i;
  79.  
  80.  i=(long)(N*val);
  81.  for(;i>0;i-=D);
  82. }
  83.  
  84. /* defines for output line pair control */
  85.  
  86. #define     C0L0()       outportb(OUTPORT, 0)        /* clock 0 latch 0 */
  87. #define     C0L1()       outportb(OUTPORT, GLATCH)   /* clock 0 latch 1 */
  88. #define     C1L0()       outportb(OUTPORT, GCLOCK)   /* clock 1 latch 0 */
  89. #define     C1L1()       outportb(OUTPORT, GCLOLAT)  /* clock 1 latch 1 */
  90.  
  91.  
  92. /* prototypes */
  93.  
  94. void Hires (void);             /* puts glove in hires mode   */
  95. void getglove (unsigned char *);    /* get data packet from glove */
  96. int  glove_ready();            /* returns 0 if not ready     */
  97.                     /* delay repeats by 2-4 ms    */
  98. unsigned char getbyte (void);        /* read byte from glove */
  99.  
  100.  
  101. /***** GLOVE DATA SPECIFICATIONS **************
  102.  
  103.  The glove_data array has been simplified. These are its functions:
  104.  
  105.  
  106. x =   X position, 3mm per number
  107. y =   Y position, 3mm per number
  108. z =   distance,  14mm per number
  109. rot = wrist twist. 0 is up 1 is slightly CW, 5 is down, 11 is slightly CCW.
  110.       About 30 to 40 degrees per count.
  111.  
  112.  Note: exact scaling of all above change with distance! Closer is higher res.
  113.  
  114. fingers = packed 2-bit values, 0 is open, 3 is (tight) fist:
  115.       Bit format: TtIiMmRr  for Thumb, Index, Middle, and Ring fingers.
  116.  
  117. keys: $FF or $80 is no key. Responds with 0 to 9 for keys "0" thru "9"
  118.       $82 = START, $83 = SEL, $0A = "A", $0B = "B", 0 is "Center"
  119.       Up,down,left,right are $0D,$0E,$0C,$0F respectively.
  120.  
  121. */
  122.  
  123. typedef struct glove_data {
  124.               signed char x,y,z,rot,fingers,keys;
  125.               } glove_data;
  126.  
  127. /*********************************************/
  128.  
  129. void main ()
  130. {
  131.  unsigned char buf[12];
  132.  glove_data   *glov;
  133.  unsigned unready;         /* number of unsuccessful tries to read glove */
  134.  
  135.  glov=(glove_data *)buf;
  136.  initgraph(&gdriver, &gmode, "d:\\tpas5\\bgidrvs\\");    /* VGA graphics, 640x480 */
  137.  cleardevice();
  138.                /* begin again here if glove crashes */
  139. restart:
  140.  Hires ();                 /* set PG into 'hires' mode */
  141.  
  142.  while(!kbhit())
  143.   {
  144.    unready = 0;             /* start polling glove */
  145.    fdelay(D2SLOW);
  146.    while(glove_ready()==0)              /* wait for glove to become ready */
  147.     {
  148.      if (unready++>500) goto restart;     /* reset mode if dead glove */
  149.      fdelay(D2SLOW);                               }
  150.  
  151.    getglove(buf);             /* read 6 byte packet */
  152.    gotoxy(1,1);                /* print xyz at scrren top */
  153.    printf("% 4d % 4d % 4d    ", 255&glov->x, 255&glov->y, 255&glov->z);
  154.                     /* print rot, fingers, keys */
  155.    printf("%-2x %-2x %-2x ", buf[3],buf[4],buf[5]);
  156.  
  157.    deglitch(glov);        /* remove spikes and jumps */
  158.    dehyst(glov);          /* add hysteresis to remove LL noise */
  159.  
  160.    drawp(glov);          /* plot x,y positions */
  161.    drawthing(glov);          /* animate glove cursor */
  162.   }
  163.  
  164.  getch();            /* exit when keyboard hit */
  165.  C0L0();            /* release glove on exit */
  166. }
  167.  
  168.  
  169.  
  170. void getglove(buf)      /* read 6 byte data packet */
  171. unsigned char *buf;
  172. {
  173.  register unsigned char *bp;
  174.  
  175.  bp = buf;
  176.  
  177.  *bp++ = getbyte ();    /* read data */
  178.  fdelay(D2BYTES);
  179.  *bp++ = getbyte ();
  180.  fdelay(D2BYTES);
  181.  *bp++ = getbyte ();
  182.  fdelay(D2BYTES);
  183.  *bp++ = getbyte ();
  184.  fdelay(D2BYTES);
  185.  *bp++ = getbyte ();
  186.  fdelay(D2BYTES);
  187.  *bp++ = getbyte ();
  188.  fdelay(D2BYTES);
  189.             /* throwaways (speeds up polling later) */
  190.  getbyte ();
  191.  fdelay(D2BYTES);
  192.  getbyte ();
  193. }
  194.  
  195.  
  196.  
  197. int glove_ready()    /* returns 1 if glove ready, 0 otherwise */
  198. {
  199.  int f;
  200.  f = getbyte();
  201.  return( (f==0xA0) ? 1 : 0);
  202. }
  203.  
  204.  
  205.  
  206. unsigned char getbyte ()         /* read a byte from glove <rolled code> */
  207. {
  208.  register int i;
  209.  register unsigned char x = 0;
  210.  
  211.  C1L0 ();                /* generate a reset (latch) pulse */
  212.  C1L1 ();
  213.  fdelay(D2BITS);            /* hold for 5 us */
  214.  C1L0 ();
  215.  
  216.  for(i=0;i<8;i++)
  217.   {
  218.    x=x<<1;
  219.    x+=((inportb(INPORT)&GDATA)>>4);
  220.    C0L0 ();
  221.    C1L0 ();  /* pulse */
  222.   }
  223.  
  224.  return(x);  /* return the byte */
  225. }
  226.  
  227.  
  228. /*  HIRES ENTRY CODES
  229. byte:
  230. 1- any value between $05 and $31
  231. 2- only $C1 and $81 work OK
  232. 3- no effect
  233. 4- no effect
  234. 5- no effect
  235. 6- only $FF works
  236. 7- seems to affect read rate slightly, 1 fastest
  237. */
  238.  
  239. int hires_code[7] = { 0x06, 0xC1, 0x08, 0x00, 0x02, 0xFF, 0x01 };
  240.  
  241.  
  242. void Hires ()    /* enter HIRES mode <rolled code- speed unimportant> */
  243. {
  244.  int i,j,k;
  245.               /* dummy read 4 bits from glove:  */
  246.  C1L0 (); C1L1 ();    /* generate a reset (latch) pulse */
  247.  fdelay(D2BITS);
  248.  C1L0 ();
  249.  
  250.  fdelay(D2BITS);
  251.  C0L0 (); C1L0 ();    /* pulse clock */
  252.  fdelay(D2BITS);
  253.  C0L0 (); C1L0 ();    /* pulse clock */
  254.  fdelay(D2BITS);
  255.  C0L0 (); C1L0 ();    /* pulse clock */
  256.  fdelay(D2BITS);
  257.  C0L0 (); C1L0 ();    /* pulse clock */
  258.  
  259.          /* handshake for command code? */
  260.  C1L0 ();
  261.  fdelay(16950);  /* 7212 us delay */
  262.  C1L1 ();
  263.  fdelay(4750);   /* 2260 us delay */
  264.  
  265.  for(i=0;i<7;i++)       /* send 7 bytes */
  266.   {
  267.    k=hires_code[i];
  268.    for(j=0;j<8;j++)     /* 8 bits per byte, MSB first */
  269.     {
  270.      if(k & 0x80) 
  271.       {
  272.        C1L1(); 
  273.        C0L1();
  274.        C1L1();
  275.       }
  276.      else
  277.       {
  278.        C1L0(); 
  279.        C0L0();
  280.        C1L0();
  281.       }
  282.      k=k<<1;
  283.      fdelay(D2BITS);
  284.     }
  285.    fdelay(D2BYTES);
  286.   }
  287.  
  288.  fdelay(1090);    /* 892 us delay (end of 7. byte) */
  289.  
  290.  C1L0 ();         /* drop the reset line */
  291.  fdelay(30000);   /* some time for the glove controller to relax */
  292.  fdelay(30000);
  293. }
  294.  
  295.  
  296.  
  297. glove_data oldbuf;    /* used to store old state for drawing */
  298.  
  299. int drawn = 0;        /* set if cursor to be erased */
  300.  
  301.  
  302. drawthing(glove_data *g)    /* draw square cursor */
  303. {
  304.  if(g->keys==2) return;        /* hold down "2" to stop drawing */
  305.  
  306.  if(drawn)            /* erase old box */
  307.   {
  308.    setcolor(0);
  309.    drawit(&oldbuf);
  310.   }
  311.  
  312.  setcolor(15);            /* draw new box */
  313.  drawit(g);
  314.  drawn = 1;
  315.  
  316.  oldbuf.x = g->x;        /* save pos'n for next erase */
  317.  oldbuf.y = g->y;
  318.  oldbuf.z = g->z;
  319. }
  320.  
  321.  
  322.  
  323. drawit(glove_data *g)        /* draw/erase box cursor */
  324. {
  325.  int x = 320+2*(g->x);        /* compute X,Y center */
  326.  int y = 240-2*(g->y);
  327.  int z = 30+(g->z);        /* size prop. to Z */
  328.  
  329.  rectangle(x-z,y-z,x+z,y+z);
  330. }
  331.  
  332.  
  333.  
  334. int xx = 0;                   /* plot position */
  335.  
  336. drawp(glove_data *g)    /* plot X,Y data to test smoothing */
  337. {
  338.  if(g->keys==4)     /* restart at left edge if "4" pressed */
  339.   {
  340.    cleardevice();
  341.    xx=0;
  342.   }
  343.  
  344.  setcolor(0);
  345.  line(xx,0,xx,479);
  346.  line(xx+1,0,xx+1,479);
  347.  setcolor(15);
  348.  line(xx,240-2*g->x,xx+1,240-2*g->x);
  349.  setcolor(12);
  350.  line(xx+1,240-2*g->y,xx+2,240-2*g->y);
  351.  xx++;
  352.  xx++;
  353.  if(xx>639)xx=0;
  354. }
  355.  
  356.  
  357.  
  358. int ox = -1000;            /* last x,y for hysterisis */
  359. int oy = -1000;
  360.  
  361.  
  362. dehyst(glove_data *g)        /* hysterisis deglitch (low noise removal) */
  363. {
  364.  int x = g->x;
  365.  int y = g->y;
  366.  
  367.  if(g->keys==0) ox = oy = 0;    /* handle recentering ("0"key or "Center") */
  368.  
  369.  if(x-ox>XHYST) ox = x-XHYST;    /* X hysterisis */
  370.  if(ox-x>XHYST) ox = x+XHYST;
  371.  
  372.  if(y-oy>YHYST) oy = y-YHYST;    /* Y hysterisis */
  373.  if(oy-y>YHYST) oy = y+YHYST;
  374.  
  375.  g->x = ox;            /* replace present X,Y data */
  376.  g->y = oy;
  377. }
  378.  
  379.  
  380.  
  381. int x1 = 0;        /* delayed 1 sample (for smoothed velocity test) */
  382. int y1 = 0;
  383. int x2 = 0;        /* delayed 2 samples */
  384. int y2 = 0;
  385. int lx = 0;        /* last good X,Y speed */
  386. int ly = 0;
  387. int lax = 0;        /* bad data "stretch" counter */
  388. int lay = 0;
  389. int lsx = 0;        /* X,Y "hold" values to replace bad data */
  390. int lsy = 0;
  391. int lcx = 0;        /* last X,Y speed for accel. calc. */
  392. int lcy = 0;
  393.  
  394.  
  395. deglitch(glove_data *g)
  396. {
  397.  int vx, vy;
  398.  
  399.  int x = g->x;
  400.  int y = g->y;
  401.  
  402.  if(g->keys==0)        /* reset on recentering ("0" or "Center" key) */
  403.   {
  404.    x1 = x2 = y1 = y2 = 0;
  405.    lx = ly = lax = lay = 0;
  406.    lsx = lsy = lcx = lcy = 0;
  407.   }
  408.  
  409.  vx = x-((x1+x2)>>1);        /* smoothed velocity */
  410.  vy = y-((y1+y2)>>1);
  411.  
  412.  x2 = x1;            /* update last values */
  413.  x1 = g->x;
  414.  
  415.  y2 = y1;
  416.  y1 = g->y;
  417.  
  418.  if(abs(lcx-vx)>XACC) lax = XXTEND;    /* check for extreme acceleration */
  419.  if (lax == 0) lx=vx;                   /* save only good velocity        */
  420.  lcx = vx;                              /* save velocity for next accel.  */
  421.  
  422.  if(abs(lcy-vy)>YACC) lay = YXTEND;    /* same deal for Y accel. */
  423.  if (lay == 0) ly=vy;
  424.  lcy = vy;
  425.  
  426.  if(lax!=0)        /* hold X pos'n if glitch */
  427.   {
  428.    g->x = lsx;
  429.    lax--;
  430.   }
  431.  
  432.  if(lay!=0)             /* hold Y pos'n if glitch */
  433.   {
  434.    lay--;
  435.    g->y = lsy;
  436.   }
  437.  
  438.  lsx = g->x;        /* save position for X,Y hold */
  439.  lsy = g->y;
  440.  
  441. /* g->y = x;*/
  442. }
  443.  
  444.